
#define Radio_llanta 4.0
#define Radio_robot 9.75
#define Pasos_motor 200
#define PI 3.14159265

#define shiftdecimal 14//7
#define shiftEq 35//28
#define MaskbitsEq 16777215
#define shiftSigno 34      //27
#define Mask20bits 1048575
#define Maskdecimal 16383//127

typedef long long int int64;

int64 lineas[STACKSIZE];
int NLINEAS;



int *p;
int *pme;
int *pml;
int yy=0;

//retorna el tipo de la instruccion
int Tipo_Instruccion(int64 inst)
{
    return inst >> shiftEq;
}

//retorna el valor de la etiqueta en la instruccion
int Mask_35b(int64 inst)
{
    int aux = inst & MaskbitsEq;
    return aux;
}

//mascara para retornar el valor del entero en la instruccion
int Mask_20b_Entero(int64 inst)
{
    int aux = (inst >> shiftdecimal) & Mask20bits;
    int a = inst >> shiftSigno;
    if( a == 1 )
        aux = -aux;
    return aux;
}

//mascara para retornar los bits que pertenecen a los decimales
int Mask_14b(int64 inst)
{
    return  inst & Maskdecimal;
}

//realiza la operacion binaria deacuerdo al tipo y chequea tipos
DATA Operacion_Binaria(int tipo,DATA v1,DATA v2)
{
     if(tipo==5 || tipo==6 || tipo==7 || tipo==8)
     {
            DATA aux;
            if(v1.tipo==0 && v2.tipo==0)
            {
                 if(tipo==5)
                    aux.di=v1.di+v2.di;
                 else if(tipo==6)
                    aux.di=v1.di-v2.di;
                 else if(tipo==7)
                    aux.di=v1.di*v2.di;
                 else if(tipo==8)
                    aux.di=v1.di/v2.di;
                 aux.tipo=0;
            }
            else if(v1.tipo==0 && v2.tipo==1)
            {
                 if(tipo==5)
                    aux.df=v1.di+v2.df;
                 else if(tipo==6)
                    aux.df=v1.di-v2.df;
                 else if(tipo==7)
                    aux.df=v1.di*v2.df;
                 else if(tipo==8)
                    aux.df=v1.di/v2.df;
                 aux.tipo=1;
            }
            else if(v1.tipo==1 && v2.tipo==0)
            {
                 if(tipo==5)
                    aux.df=v1.df+v2.di;
                 else if(tipo==6)
                    aux.df=v1.df-v2.di;
                 else if(tipo==7)
                    aux.df=v1.df*v2.di;
                 else if(tipo==8)
                    aux.df=v1.df/v2.di;
                 aux.tipo=1;
            }
            else if(v1.tipo==1 && v2.tipo==1)
            {
                 if(tipo==5)
                    aux.df=v1.df+v2.df;
                 else if(tipo==6)
                    aux.df=v1.df-v2.df;
                 else if(tipo==7)
                    aux.df=v1.df*v2.df;
                 else if(tipo==8)
                    aux.df=v1.df/v2.df;
                 aux.tipo=1;
            }
            else
                 Error(3);
            return aux;
     }
     else if((tipo==10 || tipo==11 || tipo==12 || tipo==13))
     {
          DATA aux;
          if(v1.tipo==0 && v2.tipo==0)
          {
                 if(tipo==10)
                    aux.di=(v1.di<v2.di) ? 1 : 0;
                 else if(tipo==11)
                    aux.di=(v1.di>v2.di) ? 1 : 0;
                 else if(tipo==12)
                    aux.di=(v1.di<=v2.di) ? 1 : 0;
                 else if(tipo==13)
                    aux.di=(v1.di>=v2.di) ? 1 : 0;
          }
          else if(v1.tipo==0 && v2.tipo==1)
          {
                 if(tipo==10)
                    aux.di=(v1.di<v2.df) ? 1 : 0;
                 else if(tipo==11)
                    aux.di=(v1.di>v2.df) ? 1 : 0;
                 else if(tipo==12)
                    aux.di=(v1.di<=v2.df) ? 1 : 0;
                 else if(tipo==13)
                    aux.di=(v1.di>=v2.df) ? 1 : 0;
          }
          else if(v1.tipo==1 && v2.tipo==0)
          {
                 if(tipo==10)
                    aux.di=(v1.df<v2.di) ? 1 : 0;
                 else if(tipo==11)
                    aux.di=(v1.df>v2.di) ? 1 : 0;
                 else if(tipo==12)
                    aux.di=(v1.df<=v2.di) ? 1 : 0;
                 else if(tipo==13)
                    aux.di=(v1.df>=v2.di) ? 1 : 0;
          }
          else if(v1.tipo==1 && v2.tipo==1)
          {
                 if(tipo==10)
                    aux.di=(v1.df<v2.df) ? 1 : 0;
                 else if(tipo==11)
                    aux.di=(v1.df>v2.df) ? 1 : 0;
                 else if(tipo==12)
                    aux.di=(v1.df<=v2.df) ? 1 : 0;
                 else if(tipo==13)
                    aux.di=(v1.df>=v2.df) ? 1 : 0;
          }
          else
               Error(3);
          aux.tipo=2;
          return aux;    
     }
     else if(tipo==14 || tipo==15)
     {
          DATA aux;
          if(v1.tipo==0 && v2.tipo==0)
          {
                 if(tipo==14)
                    aux.di=(v1.di == v2.di) ? 1 : 0;
                 else if(tipo==15)
                    aux.di=(v1.di != v2.di) ? 1 : 0;
          }
          else if(v1.tipo==0 && v2.tipo==1)
          {
                  if(tipo==14)
                    aux.di=(v1.di == v2.df) ? 1 : 0;
                 else if(tipo==15)
                    aux.di=(v1.di != v2.df) ? 1 : 0;
          }
          else if(v1.tipo==1 && v2.tipo==0)
          {
                  if(tipo==14)
                    aux.di=(v1.df == v2.di) ? 1 : 0;
                 else if(tipo==15)
                    aux.di=(v1.df != v2.di) ? 1 : 0;
          }
          else if(v1.tipo==1 && v2.tipo==1)
          {
                 if(tipo==14)
                    aux.di=(v1.df == v2.df) ? 1 : 0;
                 else if(tipo==15)
                    aux.di=(v1.df != v2.df) ? 1 : 0;
          }
          else if(v1.tipo==2 && v2.tipo==2)
          {
                 if(tipo==14)
                    aux.di=(v1.di == v2.di) ? 1 : 0;
                 else if(tipo==15)
                    aux.di=(v1.di != v2.di) ? 1 : 0;
          }
          else
               Error(3);
          aux.tipo=2;
          return aux;
     }
     else if(tipo==16 || tipo==17)
     {
          DATA aux;
          if(v1.tipo==2 && v2.tipo==2)
          {
                 if(tipo==16)
                    aux.di=((v1.di & v2.di)==1) ? 1 : 0;
                 else if(tipo==17)
                    aux.di=((v1.di | v2.di)==1) ? 1 : 0;
          }
          else
               Error(3);
          aux.tipo=2;
          return aux;    
     }
     
}

//realiza la operacion unaria deacuerdo al tipo y chequea tipos
DATA Operacion_Unaria(int tipo,DATA v)
{
     if(tipo==9)
     {
          if(v.tipo==0)
          {
               v.di=-v.di;
               return v;
          }
          else if(v.tipo==1)
          {
               v.df=-v.df;
               return v;
          }
          else
               Error(3);
     }
     else if(tipo==18)
     {
          if(v.tipo==2)
          {
              v.di=(v.di==0) ? 1 : 0; 
              return v;
          }
          else
              Error(3);
     }
     else
         Error(3);
}

//procesa una instruccion "jumpFalse"=tipo 19
int Proces_jumpFalse(int i,DATA v,int inst)
{
    int j;
    if(v.tipo==2)
    {
        if(v.di==0)
            j=Mask_35b(inst);
        else
            j=i+1;
    }
    else
        Error(3);
    return j;
}

int redondear(float valor)
{
    int pe=(int)valor;
    float pd=valor-pe;
    if(pd>0.5)
        return pe+1;
    else
        return pe;
}

//convierte de grados a radianes
float I_Conv_To_Radianes(int grados)
{
    float rad=grados*PI/180.0;
    return rad;
}

//convierte de grados a radianes
float F_Conv_To_Radianes(float grados)
{
    float rad=grados*PI/180.0;
    return rad;
}

//retorna los pasos necesarios para girar tantos grados
int I_Conv_To_Pasos_Giro(int grados)
{
    float radianes=I_Conv_To_Radianes(grados);
    float num=radianes*Radio_robot*Pasos_motor;
    float den=2.0*PI*Radio_llanta;
    int ps=redondear(num/den);
    return ps;
}

//retorna los pasos necesarios para girar tantos grados
int F_Conv_To_Pasos_Giro(float grados)
{
    float radianes=F_Conv_To_Radianes(grados);
    float num=radianes*Radio_robot*Pasos_motor;
    float den=2.0*PI*Radio_llanta;
    int ps=redondear(num/den);
    return ps;
}

//retorna los pasos necesarios para avanzar tanta distancia
int  I_Conv_To_Pasos(int distancia)
{

	float num = distancia*8;
    //float num=distancia*Pasos_motor;
    //float den=2*PI*Radio_llanta;
    //int ps=redondear(num/den);
    return num;
}    

//retorna los pasos necesarios para avanzar tanta distancia
int  F_Conv_To_Pasos(float distancia)
{
	float num = distancia*8;

    //float num=distancia*Pasos_motor;
    //float den=2*PI*Radio_llanta;
    //int ps=redondear(num/den);
    return redondear(num);
}

//asigna el numero de pasos a  los registros de los motores
void Enviar_To_Motores(int pasos)
{
     //printf("%i\n",pasos);
	 int i = pasos & 1073741823;//mascara de 30 bits
	 if(i != 0)
	 {
		 *pme = pasos;
		 i=0;
		 while(1)
		 {
			 //xil_printf("value %d \n\r",*pml);
			 while(*pml == 0)
			 {
				 //*p = pasos<<4;
				 i = 1;
			 }
			 //*p=0;
			 if(i==1)
				 break;
		 }
		 //*p = 0x00000000 | (pasos<<4) ;
		 *pme = 0;
	 }

	 int q;
	 for(q=0;q<1000000;q++)
			;
	 //*p = pasos<<4;


     
}

//procesa una instruccion Avanzar o retroceder de acuerdo al sentido 1 adelante
void Avanzar(DATA v, int sentido)
{
	 unsigned int r=0;
	 if(sentido == 1)
		 r = 3 << 30;
     if(v.tipo==0)
     {
    	 if(v.di < 0 && sentido == 1)
    	 {
    		 r = 0;
    		 v.di = v.di * -1;
    	 }
    	 if(v.di < 0 && sentido == 0)
    	 {
    		 r = 3 << 30;
    		 v.di = v.di * -1;

    	 }
         int ps=I_Conv_To_Pasos(v.di);
    	 r = r | ps;
         Enviar_To_Motores(r);
     }
     else if(v.tipo==1)
     {
    	 if(v.df < 0 && sentido == 1)
		 {
    		 r = 0;
    		 v.df = v.df * -1;
		 }
		 if(v.df < 0 && sentido == 0)
		 {
			 r = 3 << 30;
			 v.df = v.df * -1;
		 }
         int ps=F_Conv_To_Pasos(v.df);  // se calcula el numero de pasos
    	 r = r | (ps);
         Enviar_To_Motores(r);
     }
     else
         Error(3);


}

//procesa una instruccion de girar
void Girar(DATA v,int lado)
{
	unsigned int r = 1 << 31;
	if(lado)
		r = 1 << 30;
     if(v.tipo==0)
     {
         int ps = I_Conv_To_Pasos_Giro(v.di%360);
    	 r = r | ps;
         Enviar_To_Motores(r);
     }
     else if(v.tipo==1)
     {
    	 int gr=((int)(v.df))%360;
         int ps = F_Conv_To_Pasos_Giro(gr+(v.df-gr));
    	 r = r | ps;
         Enviar_To_Motores(r);
     }
     else
         Error(3);
}


char **split ( char *string, const char sep) {

    char       **lista;
    char       *p = string;
    int         i = 0;

    int         pos;
    const int   len = strlen (string);

    lista = (char **) malloc (sizeof (char *));
    if (lista == NULL) {                      /* Cannot allocate memory */
        return NULL;
    }

    lista[pos=0] = NULL;

    while (i <len) {

        while ((p[i] == sep) && (i <len))
            i++;

        if (i <len) {

            char **tmp = (char **) realloc (lista , (pos + 2) * sizeof (char *));
            if (tmp == NULL) {       /* Cannot allocate memory */
                free (lista);
                return NULL;
            }
            lista = tmp;
            tmp = NULL;

            lista[pos + 1] = NULL;
            lista[pos] = (char *) malloc (sizeof (char));
            if (lista[pos] == NULL) {         /* Cannot allocate memory */
                for (i = 0; i <pos; i++)
                    free (lista[i]);
                free (lista);
                return NULL;
            }

            int j = 0;
            for (i; ((p[i] != sep) && (i <len)); i++) {
                lista[pos][j] = p[i];
                j++;

                char *tmp2 = (char *) realloc (lista[pos],(j + 1) * sizeof (char));
                if (lista[pos] == NULL) {     /* Cannot allocate memory */
                    for (i = 0; i <pos; i++)
                        free (lista[i]);
                    free (lista);
                    return NULL;
                }
                lista[pos] = tmp2;
                tmp2 = NULL;
            }
            lista[pos][j] = '\0';
            pos++;
        }
    }

    return lista;
}

void LeerLineas(char **listSplit)
{
	int i;
	i=0;

	while (listSplit[i]!=NULL)
	{
		lineas[i] = atoll(listSplit[i]);
		i++;
	}
	NLINEAS = i;
}

void esperar_inicio()
{
	XGpio gpPB;
	XGpio_Initialize(&gpPB, XPAR_BUTTONS_4BIT_DEVICE_ID);
	XGpio_SetDataDirection(&gpPB, 1, 0x0000001F);
	int data;
	while(1)
	{
		  // Read the state of the push buttons
		  data = XGpio_DiscreteRead(&gpPB, 1);
		  if(data!=0)
			  break;
	}
}



